#!/usr/bin/env python3
"""
RK3588割叶机控制系统 - 主程序
功能：视频推流 + Modbus控制 + 传感器数据上传
"""
import sys
import os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

import json
import socket
import threading
import time
import subprocess
import logging
from datetime import datetime

try:
    import serial
    from pymodbus.client.sync import ModbusSerialClient
    MODBUS_AVAILABLE = True
except ImportError:
    MODBUS_AVAILABLE = False

from config import NETWORK, VIDEO, RS485, PLC_REGISTERS

# 日志配置
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s [%(levelname)s] %(message)s'
)
logger = logging.getLogger(__name__)


class RK3588Client:
    """RK3588客户端 - 极简稳定版"""
    
    def __init__(self, server_ip, server_port):
        self.server_ip = server_ip
        self.server_port = server_port
        
        # 连接状态
        self.tcp_socket = None
        self.running = False
        self.connected = False
        
        # 子进程
        self.video_process = None
        self.modbus_client = None
        
        # 数据缓存
        self.sensor_data = {}
        self.control_data = {}
    
    def init_modbus(self):
        """初始化Modbus连接"""
        if not MODBUS_AVAILABLE:
            logger.warning("⚠️ pymodbus未安装，RS485功能不可用")
            return False
        
        try:
            self.modbus_client = ModbusSerialClient(
                method='rtu',
                port=RS485['port'],
                baudrate=RS485['baudrate'],
                bytesize=RS485['bytesize'],
                parity=RS485['parity'],
                stopbits=RS485['stopbits'],
                timeout=RS485['timeout']
            )
            
            if self.modbus_client.connect():
                logger.info(f"✅ Modbus连接成功: {RS485['port']}")
                return True
            else:
                logger.error(f"❌ Modbus连接失败: {RS485['port']}")
                return False
        except Exception as e:
            logger.error(f"❌ Modbus初始化失败: {e}")
            return False
    
    def start_video(self):
        """启动视频流 - MJPEG over HTTP"""
        try:
            # 停止旧进程
            subprocess.run(['pkill', '-9', '-f', 'gst-launch'], 
                         stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
            time.sleep(0.5)
            
            # GStreamer管道 - MJPEG over TCP
            pipeline = [
                'gst-launch-1.0', '-e',
                'v4l2src', f'device={VIDEO["device"]}',
                '!', f'video/x-raw,width={VIDEO["width"]},height={VIDEO["height"]},framerate={VIDEO["fps"]}/1',
                '!', 'videoconvert',
                '!', 'jpegenc', f'quality={VIDEO["quality"]}',
                '!', 'multipartmux', 'boundary=frame',
                '!', 'tcpserversink', 'host=0.0.0.0', f'port={NETWORK["video_port"]}',
                'sync=false', 'async=false'
            ]
            
            logger.info(f"🎬 启动GStreamer管道...")
            logger.info(f"   设备: {VIDEO['device']}")
            logger.info(f"   分辨率: {VIDEO['width']}x{VIDEO['height']}@{VIDEO['fps']}fps")
            logger.info(f"   TCP端口: {NETWORK['video_port']}")
            
            self.video_process = subprocess.Popen(
                pipeline,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                bufsize=0
            )
            
            time.sleep(3)  # 增加等待时间
            
            if self.video_process.poll() is None:
                logger.info(f"✅ GStreamer进程运行中 (PID: {self.video_process.pid})")
                logger.info(f"✅ 视频流启动成功 - tcp://0.0.0.0:{NETWORK['video_port']}")
                
                # 验证端口是否监听
                time.sleep(1)
                check_port = subprocess.run(
                    ['netstat', '-tuln'],
                    capture_output=True,
                    text=True
                )
                if f':{NETWORK["video_port"]}' in check_port.stdout:
                    logger.info(f"✅ 端口{NETWORK['video_port']}已监听")
                else:
                    logger.warning(f"⚠️ 端口{NETWORK['video_port']}未在netstat中找到")
                
                return True
            else:
                _, stderr = self.video_process.communicate()
                logger.error(f"❌ GStreamer启动失败:")
                logger.error(f"   {stderr.decode()[:500]}")
                return False
                
        except Exception as e:
            logger.error(f"❌ 视频启动异常: {e}")
            import traceback
            logger.error(traceback.format_exc())
            return False
    
    def connect_server(self):
        """连接TCP服务器"""
        try:
            self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.tcp_socket.settimeout(10)
            self.tcp_socket.connect((self.server_ip, self.server_port))
            self.connected = True
            logger.info(f"✅ 服务器连接成功: {self.server_ip}:{self.server_port}")
            return True
        except Exception as e:
            logger.error(f"❌ 服务器连接失败: {e}")
            return False
    
    def read_sensors(self):
        """读取传感器数据"""
        if not self.modbus_client:
            return {}
        
        try:
            data = {}
            slave_addr = RS485['slave_address']
            
            # 读取电压、电流、功率
            result = self.modbus_client.read_holding_registers(
                PLC_REGISTERS['voltage'], 3, unit=slave_addr
            )
            if not result.isError():
                data['voltage'] = result.registers[0]
                data['current'] = result.registers[1]
                data['power'] = result.registers[2]
            
            # 读取温度数据
            result = self.modbus_client.read_holding_registers(
                PLC_REGISTERS['generator_temp'], 5, unit=slave_addr
            )
            if not result.isError():
                data['generator_temp'] = result.registers[0]
                data['engine_temp'] = result.registers[1]
                data['hot_water_temp'] = result.registers[2]
                data['cooling_temp'] = result.registers[3]
                data['box_temp'] = result.registers[4]
            
            # 读取液位
            result = self.modbus_client.read_holding_registers(
                PLC_REGISTERS['fuel_level'], 2, unit=slave_addr
            )
            if not result.isError():
                data['fuel_level'] = result.registers[0]
                data['water_level'] = result.registers[1]
            
            # 读取PLC输出状态
            result = self.modbus_client.read_holding_registers(
                PLC_REGISTERS['plc_output'], 1, unit=slave_addr
            )
            if not result.isError():
                data['plc_output'] = result.registers[0]
            
            self.sensor_data = data
            return data
            
        except Exception as e:
            logger.error(f"传感器读取失败: {e}")
            return {}
    
    def write_control(self, register, value):
        """写入控制指令"""
        if not self.modbus_client:
            return False
        
        try:
            slave_addr = RS485['slave_address']
            result = self.modbus_client.write_register(
                register, value, unit=slave_addr
            )
            return not result.isError()
        except Exception as e:
            logger.error(f"控制写入失败: {e}")
            return False
    
    def send_data(self, data_type, data):
        """发送数据到服务器"""
        if not self.connected or not self.tcp_socket:
            return False
        
        try:
            msg = {
                'type': data_type,
                'timestamp': datetime.now().isoformat(),
                'data': data
            }
            json_data = json.dumps(msg, ensure_ascii=False) + '\n'
            self.tcp_socket.send(json_data.encode('utf-8'))
            return True
        except Exception as e:
            logger.error(f"数据发送失败: {e}")
            self.connected = False
            return False
    
    def handle_command(self, cmd):
        """处理服务器命令"""
        try:
            cmd_type = cmd.get('type')
            
            if cmd_type == 'control':
                # 控制指令
                action = cmd.get('action')
                value = cmd.get('value', 0)
                
                if action in PLC_REGISTERS:
                    register = PLC_REGISTERS[action]
                    success = self.write_control(register, value)
                    
                    # 回传确认
                    self.send_data('control_ack', {
                        'action': action,
                        'value': value,
                        'success': success
                    })
                    logger.info(f"🎮 控制: {action}={value} {'✅' if success else '❌'}")
            
            elif cmd_type == 'query':
                # 查询指令 - 立即读取并返回
                sensors = self.read_sensors()
                self.send_data('sensors', sensors)
                
        except Exception as e:
            logger.error(f"命令处理失败: {e}")
    
    def receive_thread(self):
        """接收线程 - 处理服务器命令"""
        buffer = ""
        while self.running:
            try:
                if not self.connected:
                    time.sleep(1)
                    continue
                
                self.tcp_socket.settimeout(1.0)
                data = self.tcp_socket.recv(4096)
                
                if not data:
                    logger.warning("服务器断开连接")
                    self.connected = False
                    continue
                
                buffer += data.decode('utf-8')
                
                # 处理完整JSON行
                while '\n' in buffer:
                    line, buffer = buffer.split('\n', 1)
                    if line.strip():
                        try:
                            cmd = json.loads(line.strip())
                            self.handle_command(cmd)
                        except json.JSONDecodeError:
                            pass
                        
            except socket.timeout:
                continue
            except Exception as e:
                if self.running:
                    logger.error(f"接收异常: {e}")
                    self.connected = False
                    time.sleep(2)
    
    def sensor_thread(self):
        """传感器线程 - 定期上传数据"""
        while self.running:
            try:
                if self.connected and self.modbus_client:
                    sensors = self.read_sensors()
                    if sensors:
                        self.send_data('sensors', sensors)
                        logger.debug(f"📊 传感器数据上传")
                
                time.sleep(5)  # 每5秒上传一次
                
            except Exception as e:
                logger.error(f"传感器线程异常: {e}")
                time.sleep(5)
    
    def heartbeat_thread(self):
        """心跳线程"""
        while self.running:
            try:
                if self.connected:
                    video_ok = self.video_process and self.video_process.poll() is None
                    
                    self.send_data('heartbeat', {
                        'video': video_ok,
                        'modbus': self.modbus_client is not None
                    })
                
                time.sleep(10)  # 每10秒心跳
                
            except Exception as e:
                logger.error(f"心跳异常: {e}")
                time.sleep(10)
    
    def start(self):
        """启动系统"""
        logger.info("=" * 70)
        logger.info("🚀 RK3588割叶机控制系统")
        logger.info("=" * 70)
        
        # 1. 启动视频
        if not self.start_video():
            logger.error("❌ 视频启动失败")
        
        # 2. 初始化Modbus
        if not self.init_modbus():
            logger.warning("⚠️ Modbus初始化失败，控制功能不可用")
        
        # 3. 连接服务器
        retry = 0
        while retry < 5:
            if self.connect_server():
                break
            retry += 1
            time.sleep(2)
        
        if not self.connected:
            logger.error("❌ 无法连接服务器")
            return False
        
        # 4. 启动线程
        self.running = True
        
        threading.Thread(target=self.receive_thread, daemon=True).start()
        threading.Thread(target=self.sensor_thread, daemon=True).start()
        threading.Thread(target=self.heartbeat_thread, daemon=True).start()
        
        logger.info("=" * 70)
        logger.info("✅ 系统启动完成")
        logger.info(f"📹 视频: http://{self.server_ip}:{NETWORK['video_port']}")
        logger.info(f"📡 控制: TCP {self.server_ip}:{self.server_port}")
        logger.info("=" * 70)
        
        # 保持运行
        try:
            while self.running:
                time.sleep(1)
        except KeyboardInterrupt:
            logger.info("🛑 收到停止信号")
        finally:
            self.stop()
        
        return True
    
    def stop(self):
        """停止系统"""
        logger.info("🛑 正在停止...")
        self.running = False
        
        if self.video_process:
            self.video_process.terminate()
            try:
                self.video_process.wait(timeout=3)
            except:
                self.video_process.kill()
        
        if self.modbus_client:
            self.modbus_client.close()
        
        if self.tcp_socket:
            self.tcp_socket.close()
        
        subprocess.run(['pkill', '-9', '-f', 'gst-launch'],
                      stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
        
        logger.info("✅ 系统已停止")


if __name__ == "__main__":
    server_ip = sys.argv[1] if len(sys.argv) > 1 else NETWORK['server_ip']
    server_port = int(sys.argv[2]) if len(sys.argv) > 2 else NETWORK['server_port']
    
    client = RK3588Client(server_ip, server_port)
    client.start()

